import rospy
from sensor_msgs.msg import LaserScan
import numpy as np
import math

pub = rospy.Publisher('/scan_180', LaserScan, queue_size=1000)


def scan_callback(scan):
    # 获取激光点的个数
    num_points = int((scan.angle_max - scan.angle_min) / scan.angle_increment)
    # 获取前方180度激光点的个数
    num_points_180 = int(180.0 / scan.angle_increment)
    rospy.loginfo("Number of points in 180 degrees: %d", num_points_180)
    # 获取前方180度激光点的距离
    ranges_180 = scan.ranges[num_points - num_points_180:num_points]
    # 将不在前180度范围内的激光点的距离设置为infinite
    ranges_180 = np.pad(ranges_180, (0, num_points - num_points_180), 'constant', constant_values=np.inf)
    # 发布前方180度激光点的距离
    scan_180 = LaserScan()
    scan_180.header = scan.header
    scan_180.angle_min = -math.pi / 2.0
    scan_180.angle_max = math.pi / 2.0
    scan_180.angle_increment = scan.angle_increment
    scan_180.time_increment = scan.time_increment
    scan_180.scan_time = scan.scan_time
    scan_180.range_min = scan.range_min
    scan_180.range_max = scan.range_max
    scan_180.ranges = ranges_180
    pub.publish(scan_180)


if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('laser_scan_listener')
    # 创建激光点的订阅者
    rospy.Subscriber('/scan', LaserScan, scan_callback)
    # 循环等待回调函数
    rospy.spin()
